#include "queen.h"
//How to run? 
//rosrun saint_nr_9 queen phase
// phase = 1 eller 2

int phase = 0;
int global_state = START;
ros::Publisher pub_activation;
ros::Publisher pub_activation2;
ros::Publisher pub_map_path;
ros::Publisher pub_goal_area;

vector<vec2D> directions;

vector<vector<char> > search_map;

vector<Tag> tags;

int main(int argc, char **argv){
	ros::init(argc, argv, "Queen"); //Creates a node named "Queen"
	ros::NodeHandle n;
	ros::Subscriber state_sub = n.subscribe("State/Info", 1, state_update);
	pub_activation = n.advertise<std_msgs::Int32>("Signals/Activation",1);
	pub_activation2 = n.advertise<std_msgs::Int32>("Signals/Activation2",1);	
	pub_map_path = n.advertise<std_msgs::String>("Signals/map_path", 1);
	pub_goal_area = n.advertise<saint_nr_9::GoalArea>("/goal_area", 1);		
	
  if (argc != 2)
  {
       cout << "usage: QUEEN, Please number the phase 1 eller 2"<<endl;
       return 1;
  }
  
  phase = atoll(argv[1])-1;
  cout << "We are in phase " << (phase+1) << endl;
  if ( phase != 0 && phase != 1)
  {
       cout << "usage: QUEEN, Please number the phase 1 eller 2"<<endl;
       return 1;  
  }
  cout << "Queen running"<<endl;
  
  //Initialze different directions for breath first search
	directions.push_back(vec2D(0,1));
	directions.push_back(vec2D(1,0));
	directions.push_back(vec2D(0,-1));	
	directions.push_back(vec2D(-1,0));
  ros::Rate loop_rate(100);
  
  while(ros::ok())
  {
    if(phase == 0)
    {
      handle_phase_1();
    }
    else if(phase == 1)
    {
      if(global_state == START)
      {
        std_msgs::Int32 p;
        p.data = 1;
        pub_activation2.publish(p);   
      }
      //handle_phase_2();
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

void handle_phase_1()
{
  //cout << global_state << " " << (global_state == START) << endl;
  
  if(global_state == START)
  {
    std_msgs::Int32 p;
    p.data = 1;
    pub_activation.publish(p);     
  }
  else
  {
    //Hui Explore
  }
}

void handle_phase_2()
{
  load_tags(TAG_PATH_1);
  readMap(MAP_PATH);
  vector<vector<int> > distances = calculate_distance_matrix();
  std_msgs::String s;
  s.data = MAP_PATH;
  pub_map_path.publish(s);
  ros::spinOnce();
  saint_nr_9::GoalArea g;
  int minIndex = 1;
  for (int i = 2; i < distances[0].size();i++)
  {
    if(distances[0][i] > distances[0][minIndex])
    {
      minIndex = i;
    }
  }
  g.x = tags[minIndex-1].position.x-5;
  g.y = tags[minIndex-1].position.y-5;
  g.x_length = 10;
  g.y_length = 10;
  g.start_direction = tags[minIndex-1].heading;
  pub_goal_area.publish(g);  
  ros::spinOnce();
}

vector<vector<int> > calculate_distance_matrix()
{
  vector<vector<int> > distances;
  for (int i = 0; i < tags.size()+1; i++)
  {
    distances.push_back(vector<int>());
    distances[i][i] = 0;
  }
  for (int i = 0; i < tags.size()+1; i++)
  {
    for (int k = i+1; k < tags.size()+1; k++)
    {
      vec2D start;
      if(i==0)
      {
        start.x = STARTPOSITION_X;
        start.y = STARTPOSITION_Y;
      }
      else
      {
        start = tags[i-1].position;
      }
      vec2D end = tags[k-1].position;      
      int dist = calculate_distance(start, end);
      distances[i][k] = dist;
      distances[k][i] = dist;      
    }
  } 
}

void load_tags(string path)
{
  std::ifstream myfile(TAG_PATH_1.c_str());
  if (myfile.is_open())
  {
    while ( myfile.good() )
    {
      Tag t;
      myfile >> t.id >> t.position.x >> t.position.y >> t.heading;
      tags.push_back(t);
    }
    myfile.close();
  }

}

void state_update(const std_msgs::Int32::ConstPtr& state)
{
  global_state = state->data;
}

int calculate_distance(vec2D start, vec2D end)
{
  queue<dist_node> bf;
  vector<vector<bool> > besoekt;
  for (int x = 0; x < search_map.size(); x++)
  {
    besoekt.push_back(vector<bool>());
    for (int y = 0; y < search_map[x].size(); y++)
    {
      besoekt[x].push_back(false);
    }
  }
  dist_node s;
  s.pos = start;
  s.dist = 0;
  bf.push(s);
  while(!bf.empty())
  {  
    dist_node t = bf.front();
    bf.pop();
    if (t.pos.x == end.x && t.pos.y == end.y)
    {
      return t.dist;
    }
    if (t.pos.x < 0 || t.pos.x >= search_map.size()
        || t.pos.y < 0 || t.pos.y >=search_map[t.pos.x].size()
        || besoekt[t.pos.x][t.pos.y] 
        || search_map[t.pos.x][t.pos.y] != FREE_SPACE)
        continue;
    
    besoekt[t.pos.x][t.pos.y] = true;
    for (int i = 0; i < directions.size(); i++ )
    {
      dist_node ny_N;
      ny_N.pos.x = t.pos.x + directions[i].x;
      ny_N.pos.y = t.pos.y + directions[i].y;
      ny_N.dist = t.dist+1;      
      bf.push(ny_N);
    }
  }
  return -1;
}

void readMap(string path)
{
  string line;
  std::ifstream myfile (path.c_str());
  
    if (myfile.is_open())
    { 
      //printf("Hui map file opened :) \n");
      //string ha = "haha";
      //cout << ha.compare("haha")<<endl;
      
      while ( myfile.good() )
      {
        //printf("Hui map file opened :) \n");
        std::getline (myfile,line);
        //cout << line << " " << "Wallcompare: " << line.compare("WALLS") << "\n" <<endl;
        //line = "WALLS";
        //cout << line.compare("WALLS")<<endl;
        
        if (line.compare(0,5,"WALLS")==0)
        {
          //printf("Juhu \n");
          break;
        }
        //cout << line <<endl;
        search_map.push_back(vector<char>());
        for (int i = 0; i < line.size();i++)
        {
          search_map.back().push_back(line[i]);
        }
      }
      myfile.close();
    }
    
    
   /* for (int i = 0; i < search_map.size();i++)
    {
      for (int j = 0; j < search_map.at(i).size(); j++)
      {
        cout << search_map.at(i).at(j);
      }
      cout << endl;
    } */  
}

